#include "foc_control.h"

static const char *TAG = "FOC";
FOC_STRUCT foc_struct;   // 定义对象

static void Foc_Data(float Volt,uint8_t PP,int8_t DIR,float voltage_limit_max,float lowpassfilter_A);
static void Pwm_Init();
static void setPwm(float Ua, float Ub, float Uc);
static void setTorque(float Uq, float Ud, float angle_el);
static void Foc_StartTimer();
static float cal_Iq(CurrSense currsense_,float angle_el);    // 计算Iq 值

// 定时器 回调函数
static void foc_timer_callback(void* arg)
{
    // velocityOpenloop(foc_struct.motor_target);   // 开环速度控制
    // AS5600_update(&foc_struct.as5600_A);

    // float angle = AS5600_get_multi_turn_angle_rad(&foc_struct.as5600_A);
    // float mech = AS5600_get_single_turn_angle_rad(&foc_struct.as5600_A);
    // float vel = AS5600_get_velocity_rad_s(&foc_struct.as5600_A);
    // float angle_deg = AS5600_get_multi_turn_angle_deg(&foc_struct.as5600_A);
    // float mech_deg = AS5600_get_single_turn_angle_deg(&foc_struct.as5600_A);

    // ESP_LOGI("AS5600", 
    //     "Angle = %.3f rad (%.1f°), Mech = %.3f rad (%.1f°), Velocity = %.3f rad/s",
    //     angle, angle_deg, mech, mech_deg, vel);
    // postionCloseControl();
    speedCloseControl();
    // currentCloseControl();
    // currsense_get_currents(&foc_struct.currsense_A);
    // ESP_LOGI(TAG,"检测电流 A: %.2f A, B: %.2f A, C: %.2f A\n", foc_struct.currsense_A.current_a, foc_struct.currsense_A.current_b, foc_struct.currsense_A.current_c);
}



// 归一化角度到 [0,2PI]
static float _normalize_angle(float angle) {
    float a = fmodf(angle, 2 * PI);
    return (a >= 0) ? a : (a + 2 * PI);
}

// 电角度 = 机械角度 * 极对数
float _electricalAngle(float angle){
//   return  _normalizeAngle((float)(foc_struct.DIR * foc_struct.PP) * as5600_get_mechanical_angle(&foc_struct.sensor)-foc_struct.zero_electric_angle);
    return _normalize_angle((float)(foc_struct.DIR * foc_struct.PP) * angle - foc_struct.zero_electric_angle);
}
/*
    Volt    电压
    PP      极对数
    DIR     传感器方向     
    voltage_limit_max  输出最大电压值
*/
void Foc_Init(float Volt,uint8_t PP,int8_t DIR,float voltage_limit_max, float lowpassfilter_A )
{
    PIDController pid_A;
    PIDController_Init(&pid_A,0.005,0.003,0.001,100000,6.3);
    Foc_Pid_Control_Init(&pid_A);

    PIDController pid_curr;
    PIDController_Init(&pid_curr,5,200,0,100000,12.6);
    Foc_Pid_Curr_Control_Init(&pid_curr);

    Pwm_Init();
    Foc_Data(Volt,PP,DIR,voltage_limit_max,lowpassfilter_A);
    AS5600_init(&foc_struct.as5600_A, I2C_NUM_0, GPIO_NUM_19, GPIO_NUM_18);
    
    FOC_alignSensor();
    currsense_init(&foc_struct.currsense_A,0);   // 初始化采集电流的
    currsense_calibrate(&foc_struct.currsense_A);
    ESP_LOGI(TAG,"偏移电流值 A: %.2f A, B: %.2f A, C: %.2f A\n", foc_struct.currsense_A.offset_ia, foc_struct.currsense_A.offset_ib, foc_struct.currsense_A.offset_ic);
    // AS5600_init(I2C_NUM_0, GPIO_NUM_19, GPIO_NUM_18);  // SDA=19, SCL=18
    vTaskDelay(2000);
}



void Foc_Run()
{
    Foc_StartTimer();
}

void FOC_alignSensor()
{
    setTorque(3,0,_3PI_2);
    vTaskDelay(1000);
    AS5600_update(&foc_struct.as5600_A);
    float angle = AS5600_getMechanicalAngle(&foc_struct.as5600_A);
    foc_struct.zero_electric_angle = _electricalAngle(angle);
    setTorque(0,0,_3PI_2);
    ESP_LOGI(TAG,"零角度：%.2f",foc_struct.zero_electric_angle);
}


/*
    pid 控制器的初始化 
*/
void Foc_Pid_Control_Init(PIDController *control_A)  // pid 控制器
{
    ESP_LOGI(TAG,"=================");
    foc_struct.pidcontroller_A = *control_A;
    ESP_LOGI(TAG,"获取到pid参数 P=%.2f,I=%.2f,d=%.2f,ramp=%.2f, limit=%.2f",foc_struct.pidcontroller_A.P,foc_struct.pidcontroller_A.I,foc_struct.pidcontroller_A.D,foc_struct.pidcontroller_A.output_ramp,foc_struct.pidcontroller_A.limit);
}


/*
    pid 电流环控制器的初始化 
*/
void Foc_Pid_Curr_Control_Init(PIDController *control_curr)  // pid 电流环控制器
{
    ESP_LOGI(TAG,"=================");
    foc_struct.pidcontroller_Iq_curr = *control_curr;
    ESP_LOGI(TAG,"获取到pid参数 P=%.2f,I=%.2f,d=%.2f,ramp=%.2f, limit=%.2f",foc_struct.pidcontroller_Iq_curr.P,foc_struct.pidcontroller_Iq_curr.I,foc_struct.pidcontroller_Iq_curr.D,foc_struct.pidcontroller_Iq_curr.output_ramp,foc_struct.pidcontroller_Iq_curr.limit);
}


// 开环速度控制
float velocityOpenloop(float target_velocity) {
    int64_t now_us = esp_timer_get_time();  // 获取自启动以来的微秒数，返回int64_t
    float Ts = (now_us - foc_struct.open_loop_timestamp) * 1e-6f;  // 转成秒
    if (Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
    foc_struct.shaft_angle = _normalize_angle(foc_struct.shaft_angle + target_velocity * Ts);
    float Uq = foc_struct.voltage_power_supply / 3;
    setTorque(Uq, 0, _electricalAngle(foc_struct.shaft_angle));  // 开环要放开她
    foc_struct.open_loop_timestamp = now_us;  //用于计算下一个时间间隔
    return Uq;
}

// 位置闭环控制
/*
    motor_target                目标角度
    now_angle                   现在的角度 多圈 °
    now_without_angle           现在的机械角度 单圈 °
*/
void postionCloseControl()
{
    AS5600_update(&foc_struct.as5600_A);
    static float Kp = 0.133;
    float angle_s = AS5600_getAngle(&foc_struct.as5600_A);
    float angle = AS5600_getMechanicalAngle(&foc_struct.as5600_A);
    static int num = 0;
    if(++num > 100){
        ESP_LOGI(TAG,"%.2f, %.2f",angle_s,angle);
        num = 0;
    }
    
    setTorque(_constrain(Kp * (foc_struct.motor_target - foc_struct.DIR * angle_s) * 180 / PI, -6, 6), 0, _electricalAngle(angle));
    // setTorque(_constrain(Kp*(motor_target - now_angle),-6,6),0,_electricalAngle(now_without_angle));
}


 // 速度闭环控制
void speedCloseControl()
{
    AS5600_update(&foc_struct.as5600_A);    // 更新角度
    currsense_get_currents(&foc_struct.currsense_A);     // 读取电流值
    float angle = AS5600_getMechanicalAngle(&foc_struct.as5600_A);
    float velocity =  AS5600_getVelocity(&foc_struct.as5600_A);
    float velocity_lowpass =  LowPassFilter_Update(&foc_struct.lowpassfilter_A,velocity);


    float Iq_target_ = PIDController_Update(&foc_struct.pidcontroller_A,(velocity_lowpass - foc_struct.motor_target)*180/PI);
    float Iq_now_ = cal_Iq(foc_struct.currsense_A,_electricalAngle(angle));
    float Iq_now_lowpass_ = LowPassFilter_Update(&foc_struct.lowpassfilter_Iq,Iq_now_);
    float res = PIDController_Update(&foc_struct.pidcontroller_Iq_curr,(Iq_target_ - Iq_now_lowpass_));

    setTorque(res,0,_electricalAngle(angle));
    static int num = 0;
    if(num++ > 200){
        printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f\n", Iq_now_, foc_struct.motor_target, res, _electricalAngle(angle),foc_struct.pidcontroller_Iq_curr.p_out,foc_struct.pidcontroller_Iq_curr.i_out,foc_struct.pidcontroller_Iq_curr.d_out,foc_struct.pidcontroller_Iq_curr.end_out);
        num = 0;
    }

}


// 单电流闭环控制
void currentCloseControl()
{
    AS5600_update(&foc_struct.as5600_A);    // 更新角度
    currsense_get_currents(&foc_struct.currsense_A);     // 读取电流值
    float angle = AS5600_getMechanicalAngle(&foc_struct.as5600_A);
    float Iq_now_ = cal_Iq(foc_struct.currsense_A,_electricalAngle(angle));
    float Iq_now_lowpass_ = LowPassFilter_Update(&foc_struct.lowpassfilter_Iq,Iq_now_);
    float res = PIDController_Update(&foc_struct.pidcontroller_Iq_curr,(foc_struct.motor_target - Iq_now_lowpass_));
    setTorque(res,0,_electricalAngle(angle));
    static int num = 0;
    if(num++ > 200){
        // esp_log_set_timestamp_func(NULL);  // 关闭自定义时间戳函数
        // ESP_LOGI(TAG,"%f,%f,%f,%f",Iq_now_ ,foc_struct.motor_target,res,_electricalAngle(angle));
        printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f\n", Iq_now_, foc_struct.motor_target, res, _electricalAngle(angle),foc_struct.pidcontroller_Iq_curr.p_out,foc_struct.pidcontroller_Iq_curr.i_out,foc_struct.pidcontroller_Iq_curr.d_out,foc_struct.pidcontroller_Iq_curr.end_out);

        num = 0;
    }
}


static void Foc_StartTimer()
{
    const esp_timer_create_args_t timer_args = {
        .callback = &foc_timer_callback,
        .arg = NULL,
        .name = "foc_loop_timer"
    };

    ESP_ERROR_CHECK(esp_timer_create(&timer_args, &foc_struct.foc_timer));
    ESP_ERROR_CHECK(esp_timer_start_periodic(foc_struct.foc_timer, 1000));  

}



static void setPwm(float Ua, float Ub, float Uc)
{
    // 电压归一化到 [0,1]
    foc_struct.dc_a = (Ua / foc_struct.voltage_power_supply);
    foc_struct.dc_b = (Ub / foc_struct.voltage_power_supply);
    foc_struct.dc_c = (Uc / foc_struct.voltage_power_supply);

    // 约束在 0~1 范围
    foc_struct.dc_a = _constrain(foc_struct.dc_a, 0.0f, 1.0f);
    foc_struct.dc_b = _constrain(foc_struct.dc_b, 0.0f, 1.0f);
    foc_struct.dc_c = _constrain(foc_struct.dc_c, 0.0f, 1.0f);

    // 计算目标占空比（0~255）
    uint32_t duty_a = (uint32_t)(foc_struct.dc_a * 255.0f);
    uint32_t duty_b = (uint32_t)(foc_struct.dc_b * 255.0f);
    uint32_t duty_c = (uint32_t)(foc_struct.dc_c * 255.0f);

    // 设置 PWM，占空比写入寄存器
    ledc_set_duty(LEDC_MODE, LEDC_CHANNEL_0, duty_a);
    ledc_update_duty(LEDC_MODE, LEDC_CHANNEL_0);

    ledc_set_duty(LEDC_MODE, LEDC_CHANNEL_1, duty_b);
    ledc_update_duty(LEDC_MODE, LEDC_CHANNEL_1);

    ledc_set_duty(LEDC_MODE, LEDC_CHANNEL_2, duty_c);
    ledc_update_duty(LEDC_MODE, LEDC_CHANNEL_2);
}


// 通过 帕克逆变换和克拉克逆变换 计算三相输出电压
/*
    Uq          目标的Uq值
    Ud          目标的Ud值    一般给 0
    angle_el    电角度 
*/
static void setTorque(float Uq, float Ud, float angle_el)
{
    // ESP_LOGI(TAG,"Up:%f, Ud:%f angle_el:%f",Uq,Ud,angle_el);
    angle_el = _normalize_angle(angle_el + foc_struct.zero_electric_angle);
    // 帕克逆变换
    foc_struct.Ualpha = -Uq * sin(angle_el);
    foc_struct.Ubeta = Uq * cos(angle_el);

    // 克拉克逆变换
    foc_struct.Ua = _constrain(foc_struct.Ualpha + foc_struct.voltage_power_supply / 2,0,foc_struct.voltage_power_supply);
    foc_struct.Ub = _constrain((sqrt(3) * foc_struct.Ubeta - foc_struct.Ualpha) / 2 + foc_struct.voltage_power_supply / 2,0,foc_struct.voltage_power_supply);
    foc_struct.Uc = _constrain((-foc_struct.Ualpha - sqrt(3) * foc_struct.Ubeta) / 2 + foc_struct.voltage_power_supply / 2,0,foc_struct.voltage_power_supply);
    setPwm(foc_struct.Ua, foc_struct.Ub, foc_struct.Uc);
}


//通过Ia,Ib,Ic计算Iq,Id(目前仅输出Iq)
static float cal_Iq(CurrSense currsense_,float angle_el)
{
    float I_alpha = currsense_.current_a;
    float I_beta = _1_SQRT3 * currsense_.current_a + _2_SQRT3 * currsense_.current_b;

    float ct = cos(angle_el);
    float st = sin(angle_el);
    //float I_d = I_alpha * ct + I_beta * st;
    float I_q = I_beta * ct - I_alpha * st;
    return I_q;
}


static void Pwm_Init()
{
    // 设置使能引脚为高电平
    gpio_config_t io_conf = {
        .pin_bit_mask = (1ULL << MOTOR_EN_GPIO),
        .mode = GPIO_MODE_OUTPUT,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .intr_type = GPIO_INTR_DISABLE,
    };
    gpio_config(&io_conf);
    gpio_set_level(MOTOR_EN_GPIO, 1);

    // 初始化定时器
    ledc_timer_config_t ledc_timer = {
        .speed_mode = LEDC_MODE,
        .duty_resolution = PWM_RESOLUTION,
        .timer_num = LEDC_TIMER,
        .freq_hz = PWM_FREQUENCY,
        .clk_cfg = LEDC_AUTO_CLK
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));

    // 配置PWM通道
    ledc_channel_config_t ledc_channels[3] = {
        {
            .channel = LEDC_CHANNEL_0,
            .duty = PWM_DUTY,
            .gpio_num = Motor_PWM_A_GPIO,
            .speed_mode = LEDC_MODE,
            .hpoint = 0,
            .timer_sel = LEDC_TIMER
        },
        {
            .channel = LEDC_CHANNEL_1,
            .duty = PWM_DUTY,
            .gpio_num = Motor_PWM_B_GPIO,
            .speed_mode = LEDC_MODE,
            .hpoint = 0,
            .timer_sel = LEDC_TIMER
        },
        {
            .channel = LEDC_CHANNEL_2,
            .duty = PWM_DUTY,
            .gpio_num = Motor_PWM_C_GPIO,
            .speed_mode = LEDC_MODE,
            .hpoint = 0,
            .timer_sel = LEDC_TIMER
        }
    };

    for (int i = 0; i < 3; i++) {
        ESP_ERROR_CHECK(ledc_channel_config(&ledc_channels[i]));
    }
    ESP_LOGI(TAG, "PWM初始化完成");
}


static void Foc_Data(float Volt,uint8_t PP,int8_t DIR,float voltage_limit_max, float lowpassfilter_A)
{
    foc_struct.PP = PP;
    foc_struct.DIR = DIR;
    foc_struct.voltage_limit_max = voltage_limit_max;
    foc_struct.open_loop_timestamp = 0;
    foc_struct.shaft_angle = 0;
    foc_struct.voltage_power_supply = Volt;
    foc_struct.zero_electric_angle = 0;
    foc_struct.Ualpha = 0;
    foc_struct.Ubeta = 0;
    foc_struct.Ua = 0;
    foc_struct.Ub = 0;
    foc_struct.Uc = 0;
    foc_struct.dc_a = 0;
    foc_struct.dc_b = 0;
    foc_struct.dc_c = 0;
    foc_struct.open_loop_timestamp = 0;
    foc_struct.motor_target = 0;
    LowPassFilter_Init(&foc_struct.lowpassfilter_A,lowpassfilter_A);    // 配置低通滤波的参数
    LowPassFilter_Init(&foc_struct.lowpassfilter_Iq,0.05);    // 配置低通滤波的参数


    ESP_LOGI(TAG,"FOC_DATA Init success");
}